package scu.maqiang.saw;

import scu.maqiang.fes.BVPType;
import scu.maqiang.fes.FES3H8PE;
import scu.maqiang.mesh.Mesh3H8;
import scu.maqiang.mesh.MeshFormat;
import scu.maqiang.mesh.ScalarFunc;
import scu.maqiang.numeric.*;

import java.io.BufferedWriter;
import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.HashSet;

public class SawSimulator {
    public static void main(String[] args) {
        long begin = System.nanoTime();
        //1. 网格导入, 网格材料设置
        Mesh3H8 sawMesh = new Mesh3H8();
        sawMesh.readMesh(MeshFormat.msh, "SAWMesh.msh");
        sawMesh.toTecplot("SAWMesh.dat");
        sawMesh.saveMesh(MeshFormat.mesh, "SAWMesh.mesh");
        sawMesh.scale(1.0e-9, 1.0e-9, 1.0e-9);
//        System.out.println(sawMesh.getNv());
//        System.out.println(sawMesh.getNt());
//        System.out.println(sawMesh.getNb());

        //2. 材料系数设置
        double epsr0 = 8.854187817e-12;
        double E_Si = 1.6e11;
        double Nu_Si = 0.22;
        double Rho_Si = 2320;
        double Epsi_Si = 4.5;
        double lambda_Si = E_Si * Nu_Si / ((1 - 2 * Nu_Si) * (1 + Nu_Si));
        double mu_Si = E_Si /(2 * (1 +Nu_Si));
        double[][] Elatic_Si = {
                {lambda_Si + 2 * mu_Si, lambda_Si, lambda_Si, 0, 0, 0},
                {lambda_Si, lambda_Si + 2 * mu_Si, lambda_Si, 0, 0, 0},
                {lambda_Si, lambda_Si, lambda_Si + 2 * mu_Si, 0, 0, 0},
                {0, 0, 0, mu_Si, 0, 0},
                {0, 0, 0, 0, mu_Si, 0},
                {0, 0, 0, 0, 0, mu_Si}
        };

        //硅材料压电本构矩阵
        double[][] PE_Si = {
                {-Epsi_Si * epsr0, 0, 0, 0, 0, 0, 0, 0, 0},
                {0, -Epsi_Si * epsr0, 0, 0, 0, 0, 0, 0, 0},
                {0, 0, -Epsi_Si * epsr0, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, lambda_Si + 2 * mu_Si, lambda_Si, lambda_Si, 0, 0, 0},
                {0, 0, 0, lambda_Si, lambda_Si + 2 * mu_Si, lambda_Si, 0, 0, 0},
                {0, 0, 0, lambda_Si, lambda_Si, lambda_Si + 2 * mu_Si, 0, 0, 0},
                {0, 0, 0, 0, 0, 0, mu_Si, 0, 0},
                {0, 0, 0, 0, 0, 0, 0, mu_Si, 0},
                {0, 0, 0, 0, 0, 0, 0, 0, mu_Si},
        };

        //二氧化硅材料参数
        double E_SiO2 = 7.0e10;
        double Nu_SiO2 = 0.17;
        double Rho_SiO2 = 2200;
        double Epsi_SiO2 = 4.2;
        double lambda_SiO2 = E_SiO2 * Nu_SiO2 / ((1 - 2 * Nu_SiO2) * (1 + Nu_SiO2));
        double mu_SiO2 = E_SiO2 /(2 * (1 +Nu_SiO2));
        //二氧化硅材料压电本构矩阵
        double[][] PE_SiO2 = {
                {-Epsi_SiO2 * epsr0, 0, 0, 0, 0, 0, 0, 0, 0},
                {0, -Epsi_SiO2 * epsr0, 0, 0, 0, 0, 0, 0, 0},
                {0, 0, -Epsi_SiO2 * epsr0, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, lambda_SiO2 + 2 * mu_SiO2, lambda_SiO2, lambda_SiO2, 0, 0, 0},
                {0, 0, 0, lambda_SiO2, lambda_SiO2 + 2 * mu_SiO2, lambda_SiO2, 0, 0, 0},
                {0, 0, 0, lambda_SiO2, lambda_SiO2, lambda_SiO2 + 2 * mu_SiO2, 0, 0, 0},
                {0, 0, 0, 0, 0, 0, mu_SiO2, 0, 0},
                {0, 0, 0, 0, 0, 0, 0, mu_SiO2, 0},
                {0, 0, 0, 0, 0, 0, 0, 0, mu_SiO2},
        };

        //铝材料参数
        double E_Al = 7.0e10;
        double Nu_Al = 0.35;
        double Rho_Al = 2700;
        double Epsi_Al = 1.0;
        double lambda_Al = E_Al * Nu_Al / ((1 - 2 * Nu_Al) * (1 + Nu_Al));
        double mu_Al = E_Al /(2 * (1 +Nu_Al));
        //铝材料压电本构矩阵
        double[][] PE_Al = {
                {-Epsi_Al * epsr0, 0, 0, 0, 0, 0, 0, 0, 0},
                {0, -Epsi_Al * epsr0, 0, 0, 0, 0, 0, 0, 0},
                {0, 0, -Epsi_Al * epsr0, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, lambda_Al + 2 * mu_Al, lambda_Al, lambda_Al, 0, 0, 0},
                {0, 0, 0, lambda_Al, lambda_Al + 2 * mu_Al, lambda_Al, 0, 0, 0},
                {0, 0, 0, lambda_Al, lambda_Al, lambda_Al + 2 * mu_Al, 0, 0, 0},
                {0, 0, 0, 0, 0, 0, mu_Al, 0, 0},
                {0, 0, 0, 0, 0, 0, 0, mu_Al, 0},
                {0, 0, 0, 0, 0, 0, 0, 0, mu_Al},
        };

        //铜材料参数
        double E_Cu = 1.2e11;
        double Nu_Cu = 0.35;
        double Rho_Cu = 8960;
        double Epsi_Cu = 1.0;

        double per_Cu = 0.047;
        double E_Al_Cu = E_Al * (1 - per_Cu) + per_Cu * E_Cu;
        double Rho_Al_Cu = Rho_Al * (1 - per_Cu) + per_Cu * Rho_Cu;
        double lambda_Al_Cu = E_Al_Cu * Nu_Al / ((1 - 2 * Nu_Al) * (1 + Nu_Al));
        double mu_Al_Cu = E_Al_Cu /(2 * (1 +Nu_Al));
        double[][] PE_Al_Cu = {
                {-Epsi_Al * epsr0, 0, 0, 0, 0, 0, 0, 0, 0},
                {0, -Epsi_Al * epsr0, 0, 0, 0, 0, 0, 0, 0},
                {0, 0, -Epsi_Al * epsr0, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, lambda_Al_Cu + 2 * mu_Al_Cu, lambda_Al_Cu, lambda_Al_Cu, 0, 0, 0},
                {0, 0, 0, lambda_Al_Cu, lambda_Al_Cu + 2 * mu_Al_Cu, lambda_Al_Cu, 0, 0, 0},
                {0, 0, 0, lambda_Al_Cu, lambda_Al_Cu, lambda_Al_Cu + 2 * mu_Al_Cu, 0, 0, 0},
                {0, 0, 0, 0, 0, 0, mu_Al_Cu, 0, 0},
                {0, 0, 0, 0, 0, 0, 0, mu_Al_Cu, 0},
                {0, 0, 0, 0, 0, 0, 0, 0, mu_Al_Cu},
        };


        double[][] Elastic_LiTaO3 = {
                { 2.32966e+011, 4.68904e+010, 8.02342e+010, -1.10267e+010,             0,             0},
                { 4.68904e+010, 2.32966e+011, 8.02342e+010,  1.10267e+010,             0,             0},
                { 8.02342e+010, 8.02342e+010, 2.75364e+011,             0,             0,             0},
                {-1.10267e+010, 1.10267e+010,            0,  9.38995e+010,             0,             0},
                {            0,            0,            0,             0,  9.38995e+010, -1.10267e+010},
                {            0,            0,            0,             0, -1.10267e+010,   9.3038e+010}
        };
        double[][] EC_LiTaO3 = {
                {        0,         0,       0,       0, 2.59576, -1.58923},
                { -1.58923,   1.58923,       0, 2.59576,       0,        0},
                {0.0821598, 0.0821598, 1.88197,       0,       0,        0}
        };
        double Rho_LiTaO3 = 7450;
        double[][] Epsi_LiTaO3 = {{40.9, 0, 0}, {0, 40.9, 0}, {0, 0, 43.3}};
        double[][] PE_LiTaO3  = {
                {   -40.9 * epsr0,        0,         0,             0,            0,            0,             0,       2.59576,      -1.58923},
                {       0,    -40.9 * epsr0,         0,      -1.58923,      1.58923,            0,       2.59576,             0,             0},
                {       0,        0,     -43.3 * epsr0,     0.0821598,    0.0821598,      1.88197,             0,             0,             0},
                {       0, -1.58923, 0.0821598,  2.32966e+011, 4.68904e+010, 8.02342e+010, -1.10267e+010,             0,             0},
                {       0,  1.58923, 0.0821598,  4.68904e+010, 2.32966e+011, 8.02342e+010,  1.10267e+010,             0,             0},
                {       0,        0,   1.88197,  8.02342e+010, 8.02342e+010, 2.75364e+011,             0,             0,             0},
                {       0,  2.59576,         0, -1.10267e+010, 1.10267e+010,            0,  9.38995e+010,             0,             0},
                { 2.59576,        0,         0,             0,            0,            0,             0,  9.38995e+010, -1.10267e+010},
                {-1.58923,        0,         0,             0,            0,            0,             0, -1.10267e+010,   9.3038e+010}
        };

        double alpha = 0.0;
        double beta = 48 * Math.PI / 180;
        double gamma = 0.0;
        double[] xN = {Math.cos(alpha), Math.sin(alpha), 0};
        double[] yN = {-Math.sin(alpha) * Math.cos(beta), Math.cos(beta) * Math.cos(alpha), Math.sin(beta)};
        double[] x = new double[3];
        Arrays.setAll(x, i->Math.cos(gamma) * xN[i] + Math.sin(gamma) * yN[i]);
        double[] y = new double[3];
        Arrays.setAll(y, i->-Math.sin(gamma) * xN[i] + Math.cos(gamma) * yN[i]);
        double[] z = {Math.sin(beta) * Math.sin(alpha), -Math.cos(alpha) * Math.sin(beta), Math.cos(beta)};

        double[][] T = {x, y, z};
        double a11 = x[0], a12 = x[1], a13 = x[2];
        double a21 = y[0], a22 = y[1], a23 = y[2];
        double a31 = z[0], a32 = z[1], a33 = z[2];
        double[][] N = {
                {a11 * a11, a12 * a12, a13 * a13, a12 * a13, a11 * a13, a11 * a12},
                {a21 * a21, a22 * a22, a23 * a23, a22 * a23, a21 * a23, a21 * a22},
                {a31 * a31, a32 * a32, a33 * a33, a32 * a33, a31 * a33, a31 * a32},
                {2 * a21 * a31, 2 * a22 * a32, 2 * a23 * a33, (a22 * a33 + a32 * a23), (a23 * a31 + a33 * a21), (a21 * a32 + a31 * a22)},
                {2 * a31 * a11, 2 * a32 * a12, 2 * a33 * a13, (a32 * a13 + a12 * a33), (a33 * a11 + a13 * a31), (a31 * a12 + a11 * a32)},
                {2 * a11 * a21, 2 * a12 * a22, 2 * a13 * a23, (a12 * a23 + a22 * a13), (a13 * a21 + a23 * a11), (a11 * a22 + a21 * a12)}
        };
        //System.out.println(MVO.toString(T));
        //System.out.println(MVO.toString(N));

        double[][] newElastic_LiTaO3 = MVO.matmul(MVO.transpose(N), MVO.matmul(Elastic_LiTaO3, N));
        //System.out.println(MVO.toString(newElastic_LiTaO3));
        double[][] newEC_LiTaO3 = MVO.matmul(MVO.transpose(T), MVO.matmul(EC_LiTaO3, N));
        //System.out.println(MVO.toString(newEC_LiTaO3));
        double[][] newEpsi_LiTaO3 = MVO.matmul(MVO.transpose(T), MVO.matmul(Epsi_LiTaO3, T));
//        System.out.println("NewEpsi_LiTaO3");
//        System.out.println(MVO.toString(newEpsi_LiTaO3));
        double[][] newPE_LiTaO3 = new double[9][9];
        double[][] newPE_LiTaO3_Complex = new double[9][9];
        double[][] Epsi_LT = new double[3][3];
        for(int i = 0; i < 9; i++) {
            for(int j = 0; j < 9; j++) {
                if (i < 3 && j < 3) {
                    newPE_LiTaO3[i][j] = Epsi_LT[i][j] = -newEpsi_LiTaO3[i][j] * epsr0;
                    newPE_LiTaO3_Complex[i][j] = -newPE_LiTaO3[i][j] * 0.001;
                } else if (i < 3 && j >= 3) {
                    newPE_LiTaO3[i][j] = newPE_LiTaO3[j][i] = newEC_LiTaO3[i][j - 3];
                } else if (i >= 3 && j >= 3) {
                    newPE_LiTaO3[i][j] = newElastic_LiTaO3[i - 3][j - 3];
                    newPE_LiTaO3_Complex[i][j] = 0.0015 * newPE_LiTaO3[i][j];
                }
            }
        }
//        System.out.println("newPE_LiTaO3_Complex");
//        System.out.println(MVO.toString(newPE_LiTaO3_Complex));
//        System.exit(1);

        double[][] Elstic_LN = {
                {2.02897e+011,  5.29177e+010, 7.49098e+010,  8.99874e+009,            0,            0},
                {5.29177e+010,  2.02897e+011, 7.49098e+010, -8.99874e+009,            0,            0},
                {7.49098e+010,  7.49098e+010, 2.43075e+011,             0,            0,            0},
                {8.99874e+009, -8.99874e+009,            0,  5.99034e+010,            0,            0},
                {           0,             0,            0,             0, 5.99018e+010, 8.98526e+009},
                {           0,             0,            0,             0, 8.98526e+009, 7.48772e+010},
        };

        double[][] PE_LN = {
                {       0,        0,       0,       0, 3.69594, -2.53384},
                {-2.53764,  2.53764,       0, 3.69548,       0,        0},
                {0.193644, 0.193644, 1.30863,       0,       0,        0}
        };
        double Rho_LN = 4700;
        double[] Epsi_LN = {43.6 * epsr0, 43.6 * epsr0, 29.16 * epsr0};


        //空气介电常数
        double Epsi_Air = 1.0;
        double[][] PE_Air = new double[9][9];
        PE_Air[0][0] = PE_Air[1][1] = PE_Air[2][2] = -Epsi_Air * epsr0;
        double[][] Epsr_Air = new double[3][3];
        Epsr_Air[0][0] = Epsr_Air[1][1] = Epsr_Air[2][2] = -Epsi_Air * epsr0;

        //3. 组装矩阵与向量
        FES3H8PE fesPE = new FES3H8PE(sawMesh);
        fesPE.setDofsFromMeshNodeInfo();

        SRMatrix peK_real = new SRMatrix(fesPE.GetNdof());
        SRMatrix peK_imag = new SRMatrix(fesPE.GetNdof());
        SRMatrix peM = new SRMatrix(fesPE.GetNdof());
        //组装Si区域
//        fesPE.assembleStiff(new double[][][]{PE_Si}, BVPType.CONSTITUTE_MATRIX_COMMON, peK_real, 1);
//        fesPE.assembleMass(new double[]{0, Rho_Si}, BVPType.CONSIST_MASS, peM, 1);
//
//        //组装SiO2区域
//        fesPE.assembleStiff(new double[][][]{PE_SiO2}, BVPType.CONSTITUTE_MATRIX_COMMON, peK_real, 2);
//        fesPE.assembleMass(new double[]{0, Rho_SiO2}, BVPType.CONSIST_MASS, peM, 2);
//        //组装LiBaO3区域
//        fesPE.assembleStiff(new double[][][]{newPE_LiTaO3}, BVPType.CONSTITUTE_MATRIX_COMMON, peK_real, 3);
//        //组装LIBaO3区域虚部稀疏矩阵
//        fesPE.assembleStiff(new double[][][]{newPE_LiTaO3_Complex}, BVPType.CONSTITUTE_MATRIX_COMMON, peK_imag, 3);
//        fesPE.assembleMass(new double[]{0, Rho_LiTaO3}, BVPType.CONSIST_MASS, peM, 3);
//        //组装Al_Cu区域
//        fesPE.assembleStiff(new double[][][]{PE_Al_Cu}, BVPType.CONSTITUTE_MATRIX_COMMON, peK_real, 4);
//        fesPE.assembleMass(new double[]{0, Rho_Al_Cu}, BVPType.CONSIST_MASS, peM, 4);
        //peK_real.sort();
//        peK_imag.sort();
//        System.out.println(peK_imag);
//        System.out.println(peK_imag.getNNZ());
//        System.exit(1);
        //peM.sort();
        //System.out.println(peM);
        //System.out.println(peM.getNNZ());
        //System.out.println(MVO.toString(Epsr_Air));
        //System.exit(1);

        //组装Air区域
        //SRMatrix peK_Air = new SRMatrix(fesPE.GetNdof());
        fesPE.assmebleElectricalStiff(new double[][][]{Epsr_Air}, BVPType.CONSTITUTE_MATRIX_COMMON, peK_real, 0);
        peK_real.sort();
//        System.out.println(peK_real);
//        System.out.println(peK_real.getNNZ());
//        System.exit(1);

        SCMatrix peK_complex = new SCMatrix(peK_real, peK_imag);
//        peK_complex.sort();
//        System.out.println(peK_complex);
//        System.exit(1);
        peM.sort();

        //4. 周期与反周期边界条件设置
        int N_dof_K = fesPE.GetNdof();
        HashSet<Integer> electrNode = sawMesh.extractDomainNodes(4);
        System.out.println(electrNode);


        ArrayList<Integer> dof_electr_V = fesPE.getDofFromNodes(electrNode, new int[]{0});
        Collections.sort(dof_electr_V);
        System.out.println(dof_electr_V);


        ScalarFunc nodeLeftY1Line = (xyz, label, param) -> Math.abs(xyz[0]) < 10e-10 && Math.abs(xyz[1]) < Constants.Er?1.0:-1.0;
        ArrayList<Integer> nodeLeftY1 = sawMesh.getNodes(nodeLeftY1Line, null);


        ArrayList<Integer> dof_left_y1 = fesPE.getDofFromNodes(nodeLeftY1);
        Collections.sort(dof_left_y1);
        dof_left_y1.remove(1);
        dof_left_y1.remove(1);
        dof_left_y1.remove(1);
        System.out.println(dof_left_y1);
        System.out.println(dof_left_y1.size());
        //System.exit(1);

        ScalarFunc nodeRightY1Line = (xyz, label, param) -> Math.abs(xyz[0]- 8.41e-7) < 10e-10 && Math.abs(xyz[1]) < Constants.Er?1.0:-1.0;
        ArrayList<Integer> nodeRightY1 = sawMesh.getNodes(nodeRightY1Line, null);
        //System.out.println(nodeRightY1);
        //System.exit(1);
        ArrayList<Integer> dof_right_y1 = fesPE.getDofFromNodes(nodeRightY1);
        Collections.sort(dof_right_y1);
        dof_right_y1.remove(1);
        dof_right_y1.remove(1);
        dof_right_y1.remove(1);
        System.out.println(dof_right_y1);
        System.out.println(dof_right_y1.size());

        ScalarFunc buttomRegion = (xyz, label, param) -> Math.abs(xyz[2]) < Constants.Er?1.0:-1.0;
        ArrayList<Integer> buttomNode = sawMesh.getNodes(buttomRegion, null);
        ArrayList<Integer> dof_buttom_DISP = fesPE.getDofFromNodes(buttomNode, new int[]{1, 2, 3});
        System.out.println(dof_buttom_DISP);
        //System.exit(1);

        //System.out.println("OK");
        int N_dof_y1 = N_dof_K / 2;
        ArrayList<Integer> dof_other_y1 = new ArrayList<>();
        for(int i = 0; i < N_dof_y1; i++) {
            dof_other_y1.add(i);
        }
        System.out.println(dof_other_y1.toString());
        for(int i = 0, n = dof_left_y1.size(); i < n; i++) {
            dof_other_y1.set(dof_left_y1.get(i), 0);
            dof_other_y1.set(dof_right_y1.get(i), 0);
        }
        for(int i = 0, n = dof_buttom_DISP.size(); i < n; i++) {
            if (dof_buttom_DISP.get(i) < N_dof_y1) {
                dof_other_y1.set(dof_buttom_DISP.get(i), 0);
            }
        }
        for(int i = 0, n = dof_electr_V.size(); i < n; i++) {
            if(dof_electr_V.get(i) < N_dof_y1) {
                dof_other_y1.set(dof_electr_V.get(i), 0);
            }
        }
        System.out.println(dof_other_y1.toString());
        var iter = dof_other_y1.iterator();
        while(iter.hasNext()) {
            if (iter.next() == 0) {
                iter.remove();
            }
        }

        System.out.println(dof_other_y1);
        System.out.println(dof_other_y1.size());
        int N_dof_Kc = N_dof_y1 - dof_left_y1.size() - (dof_buttom_DISP.size() + dof_electr_V.size()) / 2;
        System.out.println("N_dof_Kc: " + N_dof_Kc);

        SRMatrix SS = new SRMatrix(N_dof_K, N_dof_Kc);
        double a = 0.5 * Math.sqrt(2);
        System.out.println(dof_left_y1);

        for(int i = 0, n = dof_left_y1.size(); i < n; i++) {
            SS.setElementFast(dof_left_y1.get(i), i, -1.0 * a);
            SS.setElementFast(dof_right_y1.get(i), i, a);
            SS.setElementFast(dof_left_y1.get(i) + N_dof_y1, i, -1.0 * a);
            SS.setElementFast(dof_right_y1.get(i) + N_dof_y1, i, a);
        }



        int icol = dof_left_y1.size();
        for(int i = 0, n = dof_other_y1.size(); i < n; i++) {
            SS.setElementFast(dof_other_y1.get(i), icol, 1);
            SS.setElementFast(dof_other_y1.get(i) + N_dof_y1, icol, 1);
            icol++;
        }
        int[][] dofs = fesPE.getDofs();
        double[] scaleVec = new double[N_dof_K];
        int idx = 0;
        for(int i = 0, n = dofs.length; i < n; i++) {
            scaleVec[idx++] = 1.0;
            for(int j = 1; j < dofs[i].length; j++) {
                scaleVec[idx++] = 2.698828186716008e-11;
            }
        }

//        System.out.println(SS);
//        System.out.println("SS:"+ SS.getNNZ());
//        System.exit(1);
        SRMatrix TSS = SS.transpose();
        TSS.scaleCol(scaleVec);

        SS.scaleRow(scaleVec);

        SRMatrix TSSM = TSS.mul(peM);
        SRMatrix MM = TSSM.mul(SS);

//        System.out.println(MM.displayByCol());
        SCMatrix TSSA = TSS.mul(peK_complex);
        SCMatrix AA = TSSA.mul(SS);
        double[] V = new double[N_dof_K];
        for(int i = 0; i < dof_electr_V.size(); i++) {
            V[dof_electr_V.get(i)] = -1;
        }
        //SCMatrix A = SCMatrix.Add(1.0, AA, -W * W, MM);
        System.out.println("Start computing!");
        Complex[] sol = Complex.zeros(AA.getM());
        Complex[] RHS = Complex.zeros(AA.getM());
        ArrayList<Complex> Y11_List = new ArrayList<>();
        ArrayList<Complex> Z11 = new ArrayList<>();
        ArrayList<Complex> S11 = new ArrayList<>();
        double Aperture = 15.652;
        double N_IDT = 200;
        SCMatrix A, Ac;
        Complex[] RHS1 = Complex.zeros(SS.getM());
        Complex[] Q_All = Complex.zeros(N_dof_K);
        Complex Q = new Complex(0.0, 0.0);
        Complex Y11 = new Complex(0.0, 0.0);
        Complex temp1 = new Complex(0.0, 0.0);
        Complex uint = new Complex(0, -1.0);
        int[] dofElec = new int[dof_electr_V.size()];
        idx = 0;
        for(int ele: dof_electr_V) {
            dofElec[idx++] = ele;
        }
        double freqStart = 1800, freqEnd = 2200, df = 1.0, W;
        IterSCSolver solver = new IterSCSolver();
        solver.setEps(1.0e-8);
        for(double freq = freqStart; freq <= freqEnd; freq += df) {
            System.out.println("Freq: " + freq);
            W = 2 * Math.PI * freq * 1.e6;
            A = peK_complex.add(-W * W, peM);
            Ac = AA.add(-W * W, MM);
            Ac.sort();
            //System.out.println(Ac);
            //System.out.println(Ac.getNNZ());
            //System.exit(1);
            TSS.mul(A.mul(V), RHS);
//            System.out.println(Complex.toString(RHS));
//            System.exit(1);
            solver.setMatrix(Ac);
            Complex.fill(sol, 0.0);
            solver.PCGSSSOR(RHS, sol, 1.5, 1);
//            for(int i = 0; i < sol.length; i++) {
//                System.out.println(i + "  " + sol[i]);
//            }
            //System.gc();
            //System.out.println(Complex.toString(sol));
            //System.exit(1);
            //Complex.fill(RHS1, 0.0);
            SS.mul(sol, RHS1);

            A.mul(Complex.add(RHS1, -1.0, V), Q_All);
//            System.out.println(Complex.toString(Q_All));
//            System.exit(1);
            Q.image = 0.0;
            Q.real = 0.0;
            for(int i = 0, n = dofElec.length; i < n; i++) {
                Q.real += Q_All[dofElec[i]].real;
                Q.image += Q_All[dofElec[i]].image;
            }

            //Y11 = Q * W * uint;
            Q.mul(W, temp1);
            temp1.mul(uint, Y11);
            System.out.println("Y11: " + Y11);
            Y11_List.add(Y11);
            Z11.add(Complex.div(1.0 / (N_IDT * Aperture), Y11));
            System.out.println("Z11: " + Complex.div(1.0 / (N_IDT * Aperture), Y11));
            System.exit(1);
            S11.add(Complex.add(1.0, -N_IDT * Aperture * 50, Y11).div(Complex.add(1.0, N_IDT * Aperture * 50, Y11)));
            A.clear();
            Ac.clear();
        }

        //结果输出
//        System.out.println("Y11: ");
//        double freq = 0.0;
//        for(Complex ele: Y11_List) {
//            System.out.println((freqStart + freq)+ "\t  " + ele.real + "\t  " + ele.image);
//            freq += df;
//        }
        try (BufferedWriter bw = new BufferedWriter(new FileWriter("Z11.txt"))) {
            double freq = 0.0;
            for(Complex ele: Z11) {
                bw.write((freqStart + freq)+ "\t  " + ele.norm() + "\n");
                freq += df;
            }
        } catch (IOException e) {
            e.printStackTrace();
        }
        System.out.println("Z11: ");
        double freq = freqStart;
        for(Complex ele: Z11) {
            System.out.println((freqStart + freq)+ "\t  " + ele.norm());
            freq += df;
        }

//        System.out.println("S11: ");
//        freq = 0.0;
//        for(Complex ele: S11) {
//            System.out.println((freqStart + freq)+ "\t  " + ele.real + "\t  " + ele.image);
//            freq += df;
//        }
        //System.out.println(S11);
        long end = System.nanoTime();
        System.out.println("Elapsed time: " + (end - begin) / 1.0e9 + " s");
    }
}
